#!/usr/bin/env python
import roslib; roslib.load_manifest('smc_04b')
import rospy
import serial
from std_msgs.msg import String

# Node name: driver
# Subscribes to: cmd_vel (Twist)
# Publishes to: ???

def callback(cmd):
	rospy.loginfo(rospy.get_name() + " motor command received: %s", cmd.data)

def cmd_to_port(cmd):
	s = serial.Serial('/dev/ttyUSB0', 9600)
	s.write(chr(int(cmd.data)))
	callback(cmd)	

def driver():
	rospy.init_node('driver', anonymous=True)
	rospy.Subscriber("commands", String, cmd_to_port)
	rospy.spin()

if __name__ == '__main__':
	driver()
